#include <stdio.h>
#include <winsock2.h>
#include <windows.h>
#include <signal.h>
#include <stdbool.h>

#include "ethercat_motor_control.h"

#pragma comment(lib, "ws2_32.lib")

#define PORT 8080
#define BUFFER_SIZE 4096

SOCKET g_clientSocket = INVALID_SOCKET;
CRITICAL_SECTION g_cs;
bool g_running = true;

void parse_json_and_update_motor(const char* json_str) {
    json_error_t error;
    json_t* root = json_loads(json_str, 0, &error);

    if (!root) {
        printf("Failed to parse JSON: %s\n", error.text);
        return;
    }

    json_t* json_slaveid = json_object_get(root, "slaveid");
    json_t* json_target_position = json_object_get(root, "target_position");
    json_t* json_velocity = json_object_get(root, "velocity");

    if (json_is_integer(json_slaveid)) {
        int slaveid = (int)(json_integer_value(json_slaveid));
        if (slaveid >= 1 && slaveid <= slave_count) {
            devices[slaveid - 1].target_position = json_is_integer(json_target_position) ? (int)(json_integer_value(json_target_position)) : 0;
            devices[slaveid - 1].velocity = json_is_integer(json_velocity) ? (int)(json_integer_value(json_velocity)) : 0;
            devices[slaveid - 1].is_running = 1;

            if(devices[slaveid - 1].velocity == 0 && devices[slaveid - 1].target_position == 0 ){
                printf("do stop \n");
                devices[slaveid - 1].is_running = 0;
                doStop(slaveid);
            }
            else if(devices[slaveid - 1].velocity == 0 && devices[slaveid - 1].target_position != 0 ){
                doStop1(slaveid);
            }   
            else if(devices[slaveid - 1].velocity != 0 && devices[slaveid - 1].target_position == 0 ){
                doStop2(slaveid);
            }                      
            else{
                SV630N(slaveid, devices[slaveid - 1].target_position, devices[slaveid - 1].velocity);
            }

            
        } else {
            printf("Invalid slaveid: %d\n", slaveid);
        }
    }

    json_decref(root);
}

DWORD WINAPI BackgroundThread(LPVOID lpParam) {
    char message[BUFFER_SIZE];

    while (g_running) {
        read_all_motors();
        EnterCriticalSection(&g_cs);
        if (g_clientSocket != INVALID_SOCKET) {
            int offset = 0;

            offset += snprintf(message + offset, BUFFER_SIZE - offset, "[");
            
            for (int i = 0; i < slave_count; i++) {
                offset += snprintf(message + offset, BUFFER_SIZE - offset,
                                   "%s{\"ethercatid\": %d, \"status\": %d, \"current_position\":%d, \"current_speed\":%d, \"current_torque\":%d, \"target_position\":%d}",
                                   (i > 0) ? "," : "",
                                   devices[i].ethercatid,
                                   devices[i].status,
                                   devices[i].current_position,
                                   devices[i].current_speed,
                                   devices[i].current_torque,
                                   devices[i].target_position);
            
                if (offset >= BUFFER_SIZE) {
                    fprintf(stderr, "Buffer overflow detected!\n");
                    break;
                }
            }

            offset += snprintf(message + offset, BUFFER_SIZE - offset, "] \n");
            
            send(g_clientSocket, message, offset, 0);                
        }
        LeaveCriticalSection(&g_cs);
        Sleep(100);
    }
    return 0;
}

void signal_handler(int signal) {
    if (signal == SIGINT) {
        printf("Received SIGINT, shutting down...\n");
        g_running = false;
    }
}

int main(int argc, char *argv[]){
    init_soem(argc, argv);

    WSADATA wsaData;
    SOCKET serverSocket;
    struct sockaddr_in serverAddr, clientAddr;
    int clientAddrLen = sizeof(clientAddr);
    char buffer[BUFFER_SIZE];
    int bytesReceived;

    if (WSAStartup(MAKEWORD(2, 2), &wsaData) != 0) {
        printf("WSAStartup failed.\n");
        return 1;
    }

    InitializeCriticalSection(&g_cs);

    HANDLE hThread = CreateThread(NULL, 0, BackgroundThread, NULL, 0, NULL);
    if (hThread == NULL) {
        printf("Failed to create background thread.\n");
        WSACleanup();
        return 1;
    }

    signal(SIGINT, signal_handler);

    if ((serverSocket = socket(AF_INET, SOCK_STREAM, 0)) == INVALID_SOCKET) {
        printf("Socket creation failed.\n");
        WSACleanup();
        return 1;
    }

    serverAddr.sin_family = AF_INET;
    serverAddr.sin_addr.s_addr = INADDR_ANY;
    serverAddr.sin_port = htons(PORT);

    if (bind(serverSocket, (struct sockaddr*)&serverAddr, sizeof(serverAddr)) == SOCKET_ERROR) {
        printf("Bind failed.\n");
        closesocket(serverSocket);
        WSACleanup();
        return 1;
    }

    if (listen(serverSocket, 5) == SOCKET_ERROR) {
        printf("Listen failed.\n");
        closesocket(serverSocket);
        WSACleanup();
        return 1;
    }

    printf("Server is listening on port %d...\n", PORT);

    while (g_running) {
        if ((g_clientSocket = accept(serverSocket, (struct sockaddr*)&clientAddr, &clientAddrLen)) == INVALID_SOCKET) {
            printf("Accept failed.\n");
            continue;
        }

        printf("Client connected.\n");

        while ((bytesReceived = recv(g_clientSocket, buffer, BUFFER_SIZE, 0)) > 0) {
            buffer[bytesReceived] = '\0';
            printf("Received: %s\n", buffer);
            parse_json_and_update_motor(buffer);
        }

        if (bytesReceived == SOCKET_ERROR) {
            printf("Receive failed.\n");
        }

        EnterCriticalSection(&g_cs);
        closesocket(g_clientSocket);
        g_clientSocket = INVALID_SOCKET;
        LeaveCriticalSection(&g_cs);

        printf("Client disconnected. Waiting for new connection...\n");
    }

    closesocket(serverSocket);
    WSACleanup();

    TerminateThread(hThread, 0);
    CloseHandle(hThread);

    DeleteCriticalSection(&g_cs);

    printf("Server closed.\n");
    return 0;
}